﻿

using HalconDotNet;
using LeaderCCSLaserUI.Models;
using System.Collections.ObjectModel;
using System;
using ViewROI;
using NLog;
using LeaderCCSLaserUI.Services;
using MotionRobot.Models;
using System.Threading.Tasks;
using System.Threading;
using System.Diagnostics;
using Newtonsoft.Json;
using System.IO;
using Prism.Commands;
using System.Linq;
using System.Collections.Generic;
using CsvHelper;
using System.Globalization;
using Prism.Services.Dialogs;
using System.Collections.Concurrent;
using LeaderCCSLaserUI.Data;
using LeaderCCSLaserUI.Data.Extensions;
using OxyPlot;
using OxyPlot.Axes;
using OxyPlot.Series;
using Prism.Regions;

namespace LeaderCCSLaserUI.ViewModels.Home
{
    public class RightHandViewModel : NavigationViewModel
    {
        #region 变量
        private static Logger logger = LogManager.GetCurrentClassLogger();
        private readonly IGTSCardService gTSCard;
        private readonly IPLCService pLC;
        private readonly IHIKCameraService rightCamera;

        private readonly IDialogService _dialogService;
        private readonly IGetBarcodeService getBarcodeService;
        private CancellationTokenSource source0, source1;
        bool XHomed = false, YHomed = false; bool XHomed1 = false;
        bool PauseFlag = false;
        private Param? systemParam;
        MPoint initPoint;
        bool MoveToLineStartFlag = false; int otherFinishCount = 0; bool otherLineFinish = false, otherLineFail = false, otherCameraDone = false, BarcodeScanDone = false;
        bool Start2Flag = false;
        private double mz2SafePos, mz2UpDist, mxYRunSpeed, mzRunSpeed, mlineScanSpeed, mbornH, preDelay, vxyLimit, flyPreDelay, mFlyGrabPreDelay, mXMinDist, mZLimit, mCameraGrabZPos;
        private double[,] powerLimit = new double[5, 3], speedLimit = new double[5, 3], focalLengthLimit = new double[5, 3];
        HTuple calibHomMat2D1, calibHomMat2D2;
        ConcurrentBag<VisionResult> VisionResultList;
        double vx1 = 0, vy1 = 0; VResult vresult = VResult.Null;
        string comparisonPath = string.Empty, imagePath = string.Empty;
        string CurrentBarcode = string.Empty;
        int calcToken = 0;
        private bool isVisionResultConfirm = false, isCameraScanBarcode = false, isDoubleAlBarControl = false;
        double zDistUpLimit = 0, zDistLowLimit = 0;
        #endregion
        #region 属性
        #region halcon
        private HImage cameraIamge0;
        public HImage CameraIamge0
        {
            get { return cameraIamge0; }
            set { SetProperty(ref cameraIamge0, value); }
        }
        private bool cameraRepaint0;
        public bool CameraRepaint0
        {
            get { return cameraRepaint0; }
            set { SetProperty(ref cameraRepaint0, value); }
        }
        private ObservableCollection<ROI> cameraROIList0 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList0
        {
            get { return cameraROIList0; }
            set { SetProperty(ref cameraROIList0, value); }
        }
        private HObject cameraAppendHObject0;
        public HObject CameraAppendHObject0
        {
            get { return cameraAppendHObject0; }
            set { SetProperty(ref cameraAppendHObject0, value); }
        }
        private HMsgEntry cameraAppendHMessage0;
        public HMsgEntry CameraAppendHMessage0
        {
            get { return cameraAppendHMessage0; }
            set { SetProperty(ref cameraAppendHMessage0, value); }
        }
        private Tuple<string, object> cameraGCStyle0;
        public Tuple<string, object> CameraGCStyle0
        {
            get { return cameraGCStyle0; }
            set { SetProperty(ref cameraGCStyle0, value); }
        }
        #endregion
        private int stepNum;
        public int StepNum
        {
            get { return stepNum; }
            set { SetProperty(ref stepNum, value); }
        }
        private string messageStr;
        public string MessageStr
        {
            get { return messageStr; }
            set { SetProperty(ref messageStr, value); }
        }
        private PlotModel plotModel1;
        public PlotModel PlotModel1
        {
            get { return plotModel1; }
            set { SetProperty(ref plotModel1, value); }
        }
        private string vehicleBarcode;
        public string VehicleBarcode
        {
            get { return vehicleBarcode; }
            set { SetProperty(ref vehicleBarcode, value); }
        }
        #endregion
        #region 命令
        private DelegateCommand testButtonCommand;
        public DelegateCommand TestButtonCommand =>
            testButtonCommand ?? (testButtonCommand = new DelegateCommand(ExecuteTestButtonCommand));

        void ExecuteTestButtonCommand()
        {
            //GTSCard.AxisHomeMove(gTSCard.A5);

        }
        #endregion
        #region 导航
        public override void OnNavigatedTo(NavigationContext navigationContext)
        {
            LoadParam();
            LoadParamFromDB();
            LoadCameraParam();
            if (pLC.Connected)
            {
                source0 = new CancellationTokenSource();
                CancellationToken token = source0.Token;
                Task.Run(() => Run(token), token);
                addMessage("加载完成");
            }
        }
        #endregion
        #region 构造函数
        public RightHandViewModel(IContainerProvider containerProvider) : base(containerProvider)
        {
            VisionResultList = new ConcurrentBag<VisionResult>();
            MessageStr = string.Empty;
            VehicleBarcode = string.Empty;
            PlotModel1 = new PlotModel { Title = "激光功率(右)", TitleColor = OxyColor.FromRgb(32, 32, 32) };
            PlotModel1.PlotAreaBorderColor = OxyColor.FromRgb(32, 32, 32);
            PlotModel1.TextColor = OxyColor.FromRgb(32, 32, 32);
            PlotModel1.Axes.Add(new LinearAxis { Position = AxisPosition.Bottom, MajorGridlineStyle = LineStyle.Dot, Title = "时间", TicklineColor = OxyColor.FromRgb(32, 32, 32) });
            PlotModel1.Axes.Add(new LinearAxis { Position = AxisPosition.Left, MajorGridlineStyle = LineStyle.Dot, Title = "功率（W）", Maximum = 1000, TicklineColor = OxyColor.FromRgb(32, 32, 32) });
            PlotModel1.InvalidatePlot(true);
            gTSCard = containerProvider.Resolve<IGTSCardService>("GTSCard");
            pLC = containerProvider.Resolve<IPLCService>("PLC");
            rightCamera = containerProvider.Resolve<IHIKCameraService>("RightCamera");
            
            _dialogService = containerProvider.Resolve<IDialogService>();
            getBarcodeService = containerProvider.Resolve<IGetBarcodeService>();
            aggregator.ResgiterMessage(arg =>
            {
                switch (arg.Message)
                {
                    case "AppClose":
                        {
                            if (source0 != null)
                            {
                                source0.Cancel();
                            }
                            if (source1 != null)
                            {
                                source1.Cancel();
                            }
                        }
                        break;
                    default:
                        break;
                }
            });
            aggregator.ResgiterMessage(arg =>
            {
                if (arg.Message.Contains("FinishCount1"))
                {
                    var strs = arg.Message.Split(new char[] { ';' });
                    otherFinishCount = int.Parse(strs[1]);
                }
                else if (arg.Message.Contains("CalcToken2"))
                {
                    var strs = arg.Message.Split(new char[] { ';' });
                    calcToken = int.Parse(strs[1]);
                }
                else if (arg.Message.Contains("VehicleBarcode"))
                {
                    var strs = arg.Message.Split(new char[] { ';' });
                    VehicleBarcode = strs[1];
                }
                else
                {
                    switch (arg.Message)
                    {
                        case "MoveToLineStart":
                            MoveToLineStartFlag = true;
                            break;
                        case "LineFinish1":
                            otherLineFinish = true;
                            break;
                        case "ReloadParam":
                            LoadParam();
                            LoadParamFromDB();
                            addMessage("更新参数");
                            break;
                        case "ReloadCameraParam":
                            LoadCameraParam();
                            break;
                        case "Start2":
                            Start2Flag = true;
                            break;
                        case "XHomed1":
                            XHomed1 = true;
                            break;
                        case "LineFail1":
                            otherLineFail = true;
                            break;
                        case "BarcodeScanDone1":
                            BarcodeScanDone = true;
                            break;
                        case "CameraGrabDone1":
                            otherCameraDone = true;
                            break;
                        default:
                            break;
                    }
                }

            }, "Run");            
        }
        #endregion
        #region 功能函数
        private void Run(CancellationToken token)
        {
            #region 变量
            int stepnum = 0;
            bool XAlarm = false, YAlarm = false, ZAlarm = false, X1Alarm = false, Y1Alarm = false, Z1Alarm = false, XDistAlarm = false;
            bool EMGButton = true, ResetButton = false, PauseButton = false, StartButton = false, SafeGrate = true;
            var axisX = gTSCard.A5; var axisY = gTSCard.A6; var axisZ = gTSCard.A7;
            var axisX1 = gTSCard.A1; var axisY1 = gTSCard.A2; var axisZ1 = gTSCard.A3;
            int cycleCount = 0;
            Stopwatch sw = new Stopwatch();
            bool XHomeFinishFlag = false, YHomeFinishFlag = false;
            XYZPoint curPoint = new XYZPoint();
            Queue<XYZPoint> pcsQueue = new Queue<XYZPoint>();
            int finishCount = 0;
            bool cameraGrabDone = false; int weldPointsCount = 0; double dMarkSpeed = 0, ipg_power = 0, lzDist = 0; bool LazerDone = false, cameraGrabResult = false;
            bool imageFailDone = false;int alarmDialogResult = 0;
            int[] Buf1 = new int[0]; int trackCount = 0;
            FlyGrabPoint[] pathFlyGrabPoint = new FlyGrabPoint[0];
            int imageCount = 0; bool imageCalcFinishFlag = false;
            LineSeries s1 = new LineSeries(); int plotIndex = 0;
            bool isZControlStudy = false;
            #endregion
            #region 开机
            System.Threading.Thread.Sleep(2500);//开机延时2.5秒
            addMessage("请按\"复位\"按钮");
            GTSCard.SetDo(gTSCard.A1.CardNo, 14, 0);
            GTSCard.SetDo(gTSCard.A1.CardNo, 15, 0);
            #endregion
            while (true)
            {
                if (token.IsCancellationRequested)
                {
                    return;
                }
                try
                {
                    #region IO信号
                    var eMGButton = pLC.XCoils_X0[3] && pLC.XCoils_X0[7];
                    StartButton = pLC.XCoils_X0[0] || pLC.XCoils_X0[4];
                    var resetButton = pLC.XCoils_X0[1] || pLC.XCoils_X0[5];
                    var pauseButton = pLC.XCoils_X0[2] && pLC.XCoils_X0[6];
                    var safeGrate = pLC.XCoils_X0[24] && pLC.XCoils_X0[25] && pLC.XCoils_X0[26] && pLC.XCoils_X0[27];
                    var xAlarm = GTSCard.GetAxisAlarm(axisX);
                    var yAlarm = GTSCard.GetAxisAlarm(axisY);
                    var zAlarm = GTSCard.GetAxisAlarm(axisZ);
                    var x1Alarm = GTSCard.GetAxisAlarm(axisX1);
                    var y1Alarm = GTSCard.GetAxisAlarm(axisY1);
                    var z1Alarm = GTSCard.GetAxisAlarm(axisZ1);
                    if (EMGButton != eMGButton)
                    {
                        if (!eMGButton)
                        {
                            addMessage("急停按钮按下");
                        }
                        EMGButton = eMGButton;
                    }
                    if (PauseButton != pauseButton)
                    {
                        if (!pauseButton)
                        {
                            addMessage("停止按钮按下");
                        }
                        PauseButton = pauseButton;
                    }
                    if (SafeGrate != safeGrate)
                    {
                        if (!safeGrate)
                        {
                            addMessage("安全门打开");
                        }
                        SafeGrate = safeGrate;
                    }
                    if (XAlarm != xAlarm)
                    {
                        if (xAlarm)
                        {
                            if (pLC.Connected)
                            {
                                pLC.WriteMCoil(2562, true);
                            }
                            addMessage("X轴报警");
                            if (stepnum != -2)
                            {
                                aggregator.SendMessage("暂停", "OEE");
                            }
                        }
                        XAlarm = xAlarm;
                    }
                    if (YAlarm != yAlarm)
                    {
                        if (yAlarm)
                        {
                            if (pLC.Connected)
                            {
                                pLC.WriteMCoil(2562, true);
                            }
                            addMessage("Y轴报警");
                            if (stepnum != -2)
                            {
                                aggregator.SendMessage("暂停", "OEE");
                            }
                        }
                        YAlarm = yAlarm;
                    }
                    if (ZAlarm != zAlarm)
                    {
                        if (zAlarm)
                        {
                            if (pLC.Connected)
                            {
                                pLC.WriteMCoil(2562, true);
                            }
                            addMessage("Z轴报警");
                            if (stepnum != -2)
                            {
                                aggregator.SendMessage("暂停", "OEE");
                            }
                        }
                        ZAlarm = zAlarm;
                    }
                    var _xDist = GTSCard.GetEnc(axisX) - GTSCard.GetEnc(axisX1);
                    var xDistAlarm = XHomed && XHomed1 && Math.Abs(_xDist) < mXMinDist;
                    if (XDistAlarm != xDistAlarm)
                    {
                        if (xDistAlarm)
                        {
                            GTSCard.AxisStop(axisX, 0);
                            addMessage("双头距离过近报警");
                        }
                        XDistAlarm = xDistAlarm;
                    }
                    if (!pauseButton || !safeGrate || xAlarm || yAlarm || zAlarm || x1Alarm || y1Alarm || z1Alarm || xDistAlarm)
                    {
                        PauseFlag = true;
                    }
                    else
                    {
                        if (PauseFlag)
                        {
                            if (resetButton)
                            {
                                PauseFlag = false;
                            }
                        }
                    }
                    #endregion
                    #region 外循环
                    if (!EMGButton && stepnum >= 0)
                    {
                        if (source1 != null)
                        {
                            source1.Cancel();
                        }
                        ResetAction();
                        finishCount = 0;
                        otherFinishCount = 0;
                        MoveToLineStartFlag = false;
                        Start2Flag = false;
                        sw.Stop();
                        stepnum = -2;
                        pLC.WriteMCoil(2579, false);
                        pLC.WriteMCoil(2573, false);
                        pLC.WriteMCoil(2561, false);
                    }
                    #endregion
                    #region 流程
                    switch (stepnum)
                    {
                        #region 急停
                        case -2:
                            if (EMGButton)
                            {
                                addMessage("请按\"复位\"按钮");
                                stepnum = 0;
                            }
                            break;
                        #endregion
                        #region 复位
                        case 0:
                            if (ResetButton != resetButton)
                            {
                                if (resetButton)
                                {
                                    stepnum = 1;
                                }
                                ResetButton = resetButton;
                            }
                            break;
                        case 1:
                            if (!PauseFlag)
                            {
                                //励磁
                                GTSCard.ServoOn(axisZ);
                                sw.Restart();
                                stepnum = 2;
                            }
                            break;
                        case 2:
                            if (!PauseFlag)
                            {
                                if (sw.Elapsed.TotalSeconds > 2)
                                {
                                    sw.Stop();
                                    //把编码器值复制到规划值
                                    GTSCard.AxisPosSet(axisX, GTSCard.GetEnc(axisX));
                                    GTSCard.AxisPosSet(axisY, GTSCard.GetEnc(axisY));
                                    GTSCard.AxisPosSet(axisZ, GTSCard.GetEnc(axisZ));
                                    stepnum = 3;
                                }
                            }
                            break;
                        case 3:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisZ, mz2SafePos, mzRunSpeed / 5);
                                stepnum = 4;
                            }
                            break;
                        case 4:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    GTSCard.SetEnc(0, 10, 0);//辅助编码器归零
                                    GTSCard.ServoOn(axisX);
                                    GTSCard.ServoOn(axisY);
                                    stepnum = 5;
                                    sw.Restart();
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 3;
                            }
                            break;
                        case 5://Y轴复位和X轴回原点
                            if (!PauseFlag)
                            {
                                if (sw.Elapsed.TotalSeconds > 2)
                                {
                                    sw.Stop();
                                    GTSCard.AxisPosSet(axisX, GTSCard.GetEnc(axisX));
                                    GTSCard.AxisPosSet(axisY, GTSCard.GetEnc(axisY));
                                    source1 = new CancellationTokenSource();
                                    CancellationToken token1 = source1.Token;
                                    XHomeFinishFlag = false;
                                    Task.Run(() => XHome(token1), token1).ContinueWith(t => XHomeFinishFlag = true);
                                    YHomeFinishFlag = false;
                                    Task.Run(() => YHome(token1), token1).ContinueWith(t => YHomeFinishFlag = true);
                                    stepnum = 6;
                                }
                            }
                            break;
                        case 6:
                            if (!PauseFlag)
                            {
                                if (XHomeFinishFlag && YHomeFinishFlag)
                                {
                                    aggregator.SendMessage("XHomed2", "Run");
                                    stepnum = 7;
                                }
                            }
                            break;
                        case 7:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisX, initPoint.X, mxYRunSpeed / 5);
                                GTSCard.AxisPosMove(ref axisY, initPoint.Y, mxYRunSpeed / 5);
                                GTSCard.AxisPosMove(ref axisZ, initPoint.Z, mzRunSpeed / 5);
                                stepnum = 8;
                            }
                            break;
                        case 8:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY)
                                    && GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    Debug.WriteLine("复位完成");
                                    pLC.WriteMCoil(2573, true);
                                    aggregator.SendMessage("InitFinish", "Run");
                                    pLC.WriteMCoil(2579, true);
                                    stepnum = 9;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 7;
                            }
                            break;
                        #endregion
                        #region 启动
                        case 9:
                            if (!PauseFlag)
                            {
                                if (Start2Flag)
                                {
                                    double curAEnc = GTSCard.GetEnc(0, 10) / 1000;
                                    if (Math.Abs(curAEnc) <= 0.1)//检查探高
                                    {
                                        aggregator.SendMessage("Started2", "Run");
                                        addMessage("开始工作");
                                        calcToken = 0;
                                        Start2Flag = false;
                                        otherFinishCount = 0;
                                        imageFailDone = false;
                                        MoveToLineStartFlag = false;
                                        VisionResultList.Clear();
                                        VehicleBarcode = string.Empty;
                                        otherLineFinish = false;
                                        otherLineFail = false;
                                        otherCameraDone = false;
                                        trackCount = 0;
                                        CameraIamge0 = null;
                                        CameraAppendHMessage0 = null;
                                        stepnum = 11;
                                        pLC.WriteMCoil(2579, false);
                                    }
                                    else
                                    {
                                        addMessage($"探高异常:{curAEnc:f3}");
                                        stepnum = 10009;
                                    }
                                }
                            }
                            break;
                        case 11:
                            if (!PauseFlag)
                            {
                                if (isCameraScanBarcode) 
                                {
                                    BarcodeScanDone = false;
                                    stepnum = 400;
                                }
                                else
                                {
                                    if (systemParam.IsUseVision)
                                    {
                                        stepnum = 1000;
                                    }
                                    else
                                    {
                                        stepnum = 1100;
                                    }
                                }
                             
                            }
                            break;
                        case 10009:
                            if (!PauseFlag)
                            {
                                if (ResetButton != resetButton)
                                {
                                    if (resetButton)
                                    {
                                        stepnum = 9;
                                    }
                                    ResetButton = resetButton;
                                }
                            }
                            break;
                        #endregion
                        #region 扫码
                        case 400:
                            if (!PauseFlag)
                            {
                                if (BarcodeScanDone)
                                {
                                    BarcodeScanDone = false;
                                    if (systemParam.IsUseVision)
                                    {
                                        stepnum = 1000;
                                    }
                                    else
                                    {
                                        stepnum = 1100;
                                    }
                                }
                            }
                            break;
                        #endregion
                        #region 飞拍

                        case 1000:
                            if (!PauseFlag)
                            {
                                if(MoveToLineStartFlag)
                                {
                                    MoveToLineStartFlag = false;
                                    #region 计算图1
                                    imageCalcFinishFlag = false;
                                    Task.Run(() =>
                                    {
                                        using var mdb = new MDbContext(systemParam.PartList[systemParam.PartIndex]);
                                        var _points = mdb.XYZRPoints.Where(p => p.Hand == 1 && p.IsUse).OrderBy(p => p.PcsOrder);
                                        if (_points.Any())
                                        {
                                            _points.ToList().ForEach(p =>
                                            {
                                                var visionPcs = mdb.VisionPoints.FirstOrDefault(v => v.Hand == 1 && v.PcsOrder == p.PcsOrder);
                                                if (visionPcs != null)
                                                {
                                                    var flyimage = gTSCard.ImageList.FirstOrDefault(img => img.CameraHand == visionPcs.CameraHand && img.Kind ==  visionPcs.ImageKind && img.Track == visionPcs.Track && img.ImagePcsOrder == visionPcs.ImagePcsOrder);
                                                    if (flyimage != null)
                                                    {
                                                        //0:未做模板 1:没找到 2:超限 200:PASS
                                                        VisionResult vresult = new VisionResult();
                                                        vresult.PcsOrder = p.PcsOrder;
                                                        vresult.r_name = p.Name;
                                                        while (calcToken % 2 != 1 && calcToken < 999)
                                                        {
                                                            System.Threading.Thread.Sleep(100);
                                                        }
                                                        calcToken++;
                                                        try
                                                        {
                                                            HOperatorSet.CropPart(flyimage.MImage, out var imagePart, visionPcs.Rec1Row1, visionPcs.Rec1Column1, visionPcs.Rec1Column2 - visionPcs.Rec1Column1, visionPcs.Rec1Row2 - visionPcs.Rec1Row1);

                                                            vresult.r_image = imagePart;
                                                            
                                                            VisionAction(visionPcs.ScriptFileName, imagePart, visionPcs.ModelKind, out var r_row, out var r_column, out var r_angle, out var result);
                                                            bool rflag = result.I == 1 && r_row.TupleLength() > 0 && r_column.TupleLength() > 0 && r_angle.TupleLength() > 0;
                                                            if (rflag)
                                                            {
                                                                
                                                                HOperatorSet.VectorAngleToRigid(visionPcs.ST_X, visionPcs.ST_Y, visionPcs.ST_Angle, r_row, r_column, r_angle, out var homMat2d);
                                                                HOperatorSet.AffineTransPoint2d(homMat2d, visionPcs.Target_X, visionPcs.Target_Y, out var trow1, out var tcolumn1);

                                                                HTuple _calc_qx, _calc_qy;
                                                                if (visionPcs.CameraHand == 0)
                                                                {
                                                                    HOperatorSet.AffineTransPoint2d(calibHomMat2D1, visionPcs.Rec1Row1 + trow1, visionPcs.Rec1Column1 + tcolumn1, out _calc_qx, out _calc_qy);
                                                                }
                                                                else
                                                                {
                                                                    HOperatorSet.AffineTransPoint2d(calibHomMat2D2, visionPcs.Rec1Row1 + trow1, visionPcs.Rec1Column1 + tcolumn1, out _calc_qx, out _calc_qy);
                                                                }

                                                                var calibPoint = visionPcs.CameraHand == 0 ? systemParam.LeftCameraCalib : systemParam.RightCameraCalib;
                                                                var flypoint = mdb.FlyGrabPoints.FirstOrDefault(fly => fly.Hand == visionPcs.CameraHand && fly.Kind == visionPcs.ImageKind && fly.Track == visionPcs.Track && fly.PcsOrder == visionPcs.ImagePcsOrder);
                                                                if (flypoint != null)
                                                                {
                                                                    var calc_qx = _calc_qx + flypoint.X - calibPoint.X;
                                                                    var calc_qy = _calc_qy + flypoint.Y - calibPoint.Y;
                                                                    double deltaX = (p.X - calc_qx.D) * -1;
                                                                    double deltaY = (p.Y - calc_qy.D) * -1;
                                                                    double deltaAngle = ((visionPcs.ST_Angle - r_angle) * -1).TupleDeg().D;
                                                                    if (Math.Abs(deltaX) > vxyLimit || Math.Abs(deltaY) > vxyLimit)
                                                                    {
                                                                        vresult.result = VResult.Limit;
                                                                        addMessage($"{p.Name}用模板{visionPcs.ModelKind}偏差超限! ΔX{deltaX:f1}mm,ΔY{deltaY:f1}mm");
                                                                    }
                                                                    else
                                                                    {

                                                                        vresult.r_row = trow1;
                                                                        vresult.r_column = tcolumn1;

                                                                        vresult.r_baserow = r_row;
                                                                        vresult.r_basecolumn = r_column;

                                                                        vresult.result = VResult.PASS;
                                                                        vresult.deltaX = deltaX;
                                                                        vresult.deltaY = deltaY;
                                                                        vresult.deltaAngle = deltaAngle;
                                                                    }
                                                                }
                                                                else
                                                                {
                                                                    vresult.result = VResult.NG;
                                                                }
                                                            }
                                                            else
                                                            {
                                                                vresult.result = VResult.NG;
                                                            }

                                                        }
                                                        catch (Exception ex)
                                                        {
                                                            vresult.result = VResult.NG;
                                                            addMessage(ex.Message);
                                                        }
                                                        VisionResultList.Add(vresult);
                                                        //addMessage($"{visionPcs.CameraHand}_{visionPcs.Track}_{visionPcs.ImagePcsOrder}添加数据");
                                                        aggregator.SendMessage($"CalcToken1;{calcToken}", "Run");
                                                    }
                                                    else
                                                    {
                                                        addMessage($"{visionPcs.CameraHand}_{visionPcs.Track}_{visionPcs.ImagePcsOrder}无图像");
                                                    }
                                                }
                                                else
                                                {
                                                    addMessage($"{p.PcsOrder}未设置视觉数据");
                                                }
                                            });

                                        }
                                        aggregator.SendMessage($"CalcToken1;1000", "Run");
                                    }).ContinueWith(arg => { imageCalcFinishFlag = true; });
                                    #endregion
                                    stepnum = 1007;
                                }
                            }
                            break;   
                        case 1007:
                            if (!PauseFlag)
                            {
                                if (isVisionResultConfirm)
                                {
                                    stepnum = 1020;
                                }
                                else
                                {
                                    aggregator.SendMessage("LineFinish2", "Run");
                                    stepnum = 1008;
                                }
                            }
                            break;


                        case 1008:
                            if (!PauseFlag)
                            {
                                if (otherLineFail)
                                {
                                    otherLineFail = false;
                                    stepnum = 1022;
                                }
                                else if (otherLineFinish)
                                {
                                    otherLineFinish = false;
                                    stepnum = 1100;
                                }
                            }
                            break;
                        case 1020:
                            if (!PauseFlag)
                            {
                                if (imageCalcFinishFlag)
                                {
                                    var arr = VisionResultList.ToArray();
                                    int failIndex = -1;
                                    for (int i = 0; i < arr.Length; i++)
                                    {
                                        if (arr[i].result != VResult.PASS)
                                        {
                                            failIndex = i;
                                            break;
                                        }
                                    }
                                    if (failIndex >= 0)
                                    {
                                        aggregator.SendMessage("LineFail2", "Run");
                                        imageFailDone = false;
                                        if (pLC.Connected)
                                        {
                                            pLC.WriteMCoil(2561, true);
                                        }
                                        addMessage($"{arr[failIndex].r_name}:视觉异常");
                                        System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                                        {
                                            DialogParameters param = new DialogParameters();
                                            param.Add("PointName", arr[failIndex].r_name);
                                            param.Add("Image", arr[failIndex].r_image.CopyObj(1, -1));
                                            _dialogService.Show("VisionAlarmDialog", param, arg =>
                                            {
                                                imageFailDone = true;
                                            });
                                        }));
                                        stepnum = 1021;
                                    }
                                    else
                                    {
                                        aggregator.SendMessage("LineFinish2", "Run");
                                        stepnum = 1008;
                                    }
                                }
                            }
                            break;
                        case 1021:
                            if (!PauseFlag)
                            {
                                if (imageFailDone)
                                {
                                    imageFailDone = false;
                                    if (pLC.Connected)
                                    {
                                        pLC.WriteMCoil(2561, false);
                                    }
                                    stepnum = 1022;
                                }
                            }
                            break;

                        case 1022:
                            if (!PauseFlag && otherCameraDone)
                            {
                                GTSCard.AxisPosMove(ref axisX, initPoint.X, mxYRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, initPoint.Y, mxYRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ, initPoint.Z, mzRunSpeed);
                                stepnum = 1023;
                            }
                            break;
                        case 1023:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY)
                                    && GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    aggregator.SendMessage($"FinishCount2;999", "Run");
                                    stepnum = 1024;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 1022;
                            }
                            break;
                        case 1024:
                            if (!PauseFlag)
                            {
                                if (otherFinishCount == 999)
                                {
                                    aggregator.SendMessage("InitFinish", "Run");
                                    pLC.WriteMCoil(2579, true);
                                    addMessage("工作结束:视觉异常");
                                    stepnum = 9;
                                }
                            }
                            break;
                        #endregion
                        #region 跑点
                        case 1100:
                            if (!PauseFlag)
                            {
                                finishCount = 0;

                                pcsQueue.Clear();
                                using var mdb = new MDbContext(systemParam.PartList[systemParam.PartIndex]);
                                var points = mdb.XYZRPoints.Where(p => p.Hand == 1).OrderBy(p => p.PcsOrder);
                                if (points.Any())
                                {
                                    points.ToList().ForEach(p =>
                                    {
                                        pcsQueue.Enqueue(p);
                                    });
                                }
                                weldPointsCount = pcsQueue.Count();
                                var _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "IsZControlStudy");
                                if (_mParam != null)
                                {
                                    if (bool.TryParse(_mParam.Value, out var _v))
                                    {
                                        isZControlStudy = _v;
                                    }
                                }
                                if (pLC.Connected && weldPointsCount > 0)
                                {
                                    pLC.WriteYCoil(19, true);
                                }
                                stepnum = 11101;
                            }
                            break;
                        case 11101:
                            {
                                if (pcsQueue.TryPeek(out var _pcs))
                                {
                                    curPoint = _pcs;
                                    if (curPoint.IsUse)
                                    {
                                        stepnum = 11102;
                                    }
                                    else
                                    {
                                        stepnum = 1101;
                                    }
                                }
                                else
                                {
                                    stepnum = 1101;
                                }
                            }
                            break;
                        case 11102://先跑过去
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisX, curPoint.X, mxYRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, curPoint.Y, mxYRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ, mz2SafePos, mzRunSpeed);
                                stepnum = 11103;
                            }
                            break;
                        case 11103:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY) && GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    stepnum = 1101;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 11102;
                            }
                            break;
                
                        case 1101:
                            {
                                if (pcsQueue.TryPeek(out var _pcs))
                                {
                                    curPoint = pcsQueue.Dequeue();
                                    ipg_power = 0;
                                    dMarkSpeed = 0;
                                    lzDist = mbornH;
                                 
                                    stepnum = 1102;
                                }
                                else
                                {
                                    aggregator.SendMessage($"FinishCount2;999", "Run");
                                    if (pLC.Connected)
                                    {
                                        pLC.WriteYCoil(19, false);
                                    }
                                    stepnum = 1200;
                                    //完成
                                }
                            }
                            break;
                        case 1102:
                            if (!PauseFlag)
                            {
                                //addMessage($"finishCount:{finishCount};otherFinishCount:{otherFinishCount};VisionResultList:{VisionResultList.Count}");
                                if (finishCount <= otherFinishCount &&
                                    (!systemParam.IsUseVision || !curPoint.IsUse || VisionResultList.Count > 0))
                                {
                                    vresult = VResult.Null;
                                    vx1 = 0;
                                    vy1 = 0;
                                    if (curPoint.IsUse)
                                    {
                                        if (systemParam.IsUseVision)
                                        {
                                            var vp = VisionResultList.FirstOrDefault(p => p.PcsOrder == curPoint.PcsOrder);
                                            if (vp != null)
                                            {
                                                vresult = vp.result;
                                                if (vp.result == VResult.PASS)
                                                {
                                                    vx1 = vp.deltaX;
                                                    vy1 = vp.deltaY;
                                                    stepnum = 1105;
                                                }
                                                else
                                                {
                                                    imageFailDone = false;
                                                    if (pLC.Connected)
                                                    {
                                                        pLC.WriteMCoil(2561, true);
                                                    }
                                                    //视觉处理失败的
                                                    System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                                                    {
                                                        DialogParameters param = new DialogParameters();
                                                        param.Add("PointName", curPoint.Name);
                                                        param.Add("Image", vp.r_image.CopyObj(1, -1));
                                                        _dialogService.Show("VisionAlarmDialog", param, arg =>
                                                        {
                                                            if (arg.Result == ButtonResult.Yes)
                                                            {
                                                                try
                                                                {
                                                                    var row = arg.Parameters.GetValue<HTuple>("row");
                                                                    var column = arg.Parameters.GetValue<HTuple>("column");
                                                                    using var mdb = new MDbContext(systemParam.PartList[systemParam.PartIndex]);
                                                                    var visionPcs = mdb.VisionPoints.FirstOrDefault(v => v.Hand == 1 && v.PcsOrder == curPoint.PcsOrder);
                                                                    if (visionPcs != null)
                                                                    {
                                                                        HTuple _calc_qx, _calc_qy;
                                                                        if (visionPcs.CameraHand == 0)
                                                                        {
                                                                            HOperatorSet.AffineTransPoint2d(calibHomMat2D1, visionPcs.Rec1Row1 + row, visionPcs.Rec1Column1 + column, out _calc_qx, out _calc_qy);
                                                                        }
                                                                        else
                                                                        {
                                                                            HOperatorSet.AffineTransPoint2d(calibHomMat2D2, visionPcs.Rec1Row1 + row, visionPcs.Rec1Column1 + column, out _calc_qx, out _calc_qy);
                                                                        }
                                                                        var calibPoint = visionPcs.CameraHand == 0 ? systemParam.LeftCameraCalib : systemParam.RightCameraCalib;
                                                                        var flypoint = mdb.FlyGrabPoints.FirstOrDefault(fly => fly.Hand == visionPcs.CameraHand && fly.Kind == visionPcs.ImageKind && fly.Track == visionPcs.Track && fly.PcsOrder == visionPcs.ImagePcsOrder);
                                                                        if (flypoint != null)
                                                                        {
                                                                            var calc_qx = _calc_qx + flypoint.X - calibPoint.X;
                                                                            var calc_qy = _calc_qy + flypoint.Y - calibPoint.Y;
                                                                            double deltaX = (curPoint.X - calc_qx.D) * -1;
                                                                            double deltaY = (curPoint.Y - calc_qy.D) * -1;
                                                                            if (Math.Abs(deltaX) > vxyLimit || Math.Abs(deltaY) > vxyLimit)
                                                                            {
                                                                                vresult = VResult.Limit;
                                                                                vx1 = 0;
                                                                                vy1 = 0;
                                                                                addMessage($"手动{curPoint.Name}偏差超限! ΔX{deltaX:f1}mm,ΔY{deltaY:f1}mm");
                                                                            }
                                                                            else
                                                                            {
                                                                                vresult = VResult.PASS;
                                                                                vx1 = deltaX;
                                                                                vy1 = deltaY;
                                                                            }
                                                                        }
                                                                        else
                                                                        {
                                                                            vresult = VResult.Null;
                                                                            vx1 = 0;
                                                                            vy1 = 0;
                                                                        }
                                                                    }
                                                                }
                                                                catch (Exception ex)
                                                                {
                                                                    addMessage(ex.Message);
                                                                }

                                                            }
                                                            imageFailDone = true;
                                                        });
                                                    }));
                                                    stepnum = 11105;
                                                }
                                                System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                                                {
                                                    try
                                                    {
                                                        CameraIamge0 = new HImage(vp.r_image.CopyObj(1, -1));
                                                        if (vp.result == VResult.PASS)
                                                        {
                                                            HOperatorSet.GenCrossContourXld(out var cross1, vp.r_baserow, vp.r_basecolumn, 24, 0);
                                                            CameraGCStyle0 = new Tuple<string, object>("Color", "red");
                                                            CameraAppendHObject0 = cross1;

                                                            HOperatorSet.GenCrossContourXld(out var cross, vp.r_row, vp.r_column, 24, 0);
                                                            CameraGCStyle0 = new Tuple<string, object>("Color", "green");
                                                            CameraAppendHObject0 = cross;

                                                            CameraAppendHMessage0 = new HMsgEntry($"{curPoint.Name}\nΔX:{vp.deltaX:f3}mm\nΔY:{vp.deltaY:f3}mm\nΔR:{vp.deltaAngle:f2}°",
                                                                40, 40, "orange red", "window", "box", "false", 24, "mono", "true", "false");
                                                        }
                                                    }
                                                    catch { }
                                                }));

                                            }
                                            else
                                            {
                                                CameraIamge0 = null;
                                                addMessage($"{curPoint.Name} 视觉未设置");
                                                stepnum = 1105;
                                            }
                                        }
                                        else
                                        {
                                            stepnum = 1105;
                                        }
                                    }
                                    else
                                    {
                                        stepnum = 1105;
                                    }
                                }
                            }
                            break;
                        case 11105://先运动，避免两头过近
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisX, curPoint.X + curPoint.X1, mxYRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, curPoint.Y + curPoint.Y1, mxYRunSpeed);
                                stepnum = 11106;
                            }
                            break;
                        case 11106:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY))
                                {
                                    stepnum = 11107;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                stepnum = 11105;
                            }
                            break;
                        case 11107:
                            if (!PauseFlag)
                            {
                                if (imageFailDone)
                                {
                                    imageFailDone = false;
                                    if (pLC.Connected)
                                    {
                                        pLC.WriteMCoil(2561, false);
                                    }
                                    stepnum = 1105;
                                }
                            }
                            break;
                        case 1105:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisX, curPoint.X + vx1 + curPoint.X1, mxYRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, curPoint.Y + vy1 + curPoint.Y1, mxYRunSpeed);
                                stepnum = 1106;
                            }
                            break;
                        case 1106:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY))
                                {
                                    if (curPoint.IsUse)
                                    {
                                        stepnum = 1107;
                                    }
                                    else
                                    {
                                        stepnum = 1120;
                                    }
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                stepnum = 1105;
                            }
                            break;
                        case 1107:
                            if (!PauseFlag)
                            {
                                double acc = axisZ.Acc * systemParam.AccMul;
                                GTSCard.AxisPosMove(ref axisZ, curPoint.Z, mzRunSpeed, acc);
                                stepnum = 11108;
                            }
                            break;
                        case 11108:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    double curAEnc = GTSCard.GetEnc(0, 10) / 1000;
                                    double curH = lzDist = mbornH - curAEnc;
                                    double deltaH = curPoint.H - curH;

                                    bool zOK = true;
                                    double diffV1 = 0;
                                    if (isZControlStudy && VehicleBarcode != string.Empty)
                                    {
                                        //学习点位
                                        using var mdb = new MDbContext(systemParam.PartList[systemParam.PartIndex]);
                                        var zpoint = mdb.ZControlPoints.FirstOrDefault(z => z.Hand == 1 && z.Barcode == VehicleBarcode && z.PcsOrder == curPoint.PcsOrder);
                                        if (zpoint != null)
                                        {
                                            zpoint.ZPos = Math.Round(GTSCard.GetEnc(axisZ), 3);
                                            zpoint.Dist = Math.Round(curAEnc, 3);
                                            zpoint.UpLimit = zDistUpLimit;
                                            zpoint.LowLimit = zDistLowLimit;
                                        }
                                        else
                                        {
                                            mdb.ZControlPoints.Add(new ZControlPoint
                                            {
                                                Barcode = VehicleBarcode,
                                                Hand = 1,
                                                PcsOrder = curPoint.PcsOrder,
                                                Name = curPoint.Name,
                                                ZPos = Math.Round(GTSCard.GetEnc(axisZ), 3),
                                                Dist = Math.Round(curAEnc, 3),
                                                UpLimit = zDistUpLimit,
                                                LowLimit = zDistLowLimit
                                            });
                                        }
                                        mdb.SaveChanges();
                                    }
                                    else
                                    {
                                        if (isDoubleAlBarControl)
                                        {
                                            //比较Z轴高度
                                            using var mdb = new MDbContext(systemParam.PartList[systemParam.PartIndex]);
                                            var zpoint = mdb.ZControlPoints.FirstOrDefault(z => z.Hand == 1 && z.Barcode == VehicleBarcode && z.PcsOrder == curPoint.PcsOrder);
                                            if (zpoint != null)
                                            {
                                                var _deltaZ = (zpoint.ZPos - GTSCard.GetEnc(axisZ)) * -1;
                                                var _deltaH = (zpoint.Dist - (curAEnc - _deltaZ)) * -1;
                                                diffV1 = _deltaH;
                                                if (_deltaH > zpoint.UpLimit)
                                                {
                                                    //压多了
                                                    zOK = false;
                                                }
                                                else if (_deltaH < zpoint.LowLimit)
                                                {
                                                    //压少了
                                                    zOK = false;
                                                }
                                            }
                                            else
                                            {
                                                //没查到
                                                zOK = false;
                                            }
                                        }
                                    }

                                    if (zOK && (vresult == VResult.PASS || !systemParam.IsUseVision))
                                    {
                                        double zTargetPos = GTSCard.GetEnc(axisZ) + deltaH;
                                        GTSCard.AxisPosMove(ref axisZ, zTargetPos, mzRunSpeed, axisZ.Acc * systemParam.AccMul);
                                        
                                        stepnum = 11109;
                                    }
                                    else
                                    {
                                        if (!zOK)
                                        {
                                            string alarmstr = $"点{curPoint.Name}高度偏:{diffV1:F3}mm,超限";
                                            addMessage(alarmstr);
                                            imageFailDone = false;
                                            if (pLC.Connected)
                                            {
                                                pLC.WriteMCoil(2561, true);
                                            }
                                            System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                                            {
                                                DialogParameters param = new DialogParameters();
                                                param.Add("Message", alarmstr + "\n按\"启动\"焊接，按\"复位\"不焊。");
                                                _dialogService.Show("MessageAlarmDialog", param, arg =>
                                                {
                                                    if (arg.Result == ButtonResult.Yes)
                                                    {
                                                        alarmDialogResult = 1;
                                                    }
                                                    else
                                                    {
                                                        alarmDialogResult = 0;
                                                    }
                                                    imageFailDone = true;
                                                });
                                            }));
                                            stepnum = 11120;
                                        }
                                        else
                                            stepnum = 1110;
                                    }
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 1107;
                            }
                            break;
                        case 11120:
                            if (imageFailDone)
                            {
                                if (pLC.Connected)
                                {
                                    pLC.WriteMCoil(2561, false);
                                }
                                if (alarmDialogResult == 1)
                                {
                                    double curAEnc = GTSCard.GetEnc(0, 10) / 1000;
                                    double curH = lzDist = mbornH - curAEnc;
                                    double deltaH = curPoint.H - curH;
                                    double zTargetPos = GTSCard.GetEnc(axisZ) + deltaH;
                                    GTSCard.AxisPosMove(ref axisZ, zTargetPos, mzRunSpeed, axisZ.Acc * systemParam.AccMul);

                                    stepnum = 11109;
                                }
                                else
                                {
                                    stepnum = 1110;
                                }
                            }
                            break;
                        case 11109:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    sw.Restart();
                                    stepnum = 1108;
                                }
                            }
                            break;
                        case 1108:
                            if (!PauseFlag)
                            {
                                if (sw.Elapsed.TotalSeconds >= preDelay)//等轴到位
                                {
                                    sw.Stop();
                                    if (systemParam.IsSimulate)
                                    {
                                        sw.Restart();
                                    }
                                    else
                                    {
                                        LazerDone = false;
                                        ipg_power = 0;
                                        Task.Run(() =>
                                        {
                                            dMarkSpeed = 0;


                                            switch (systemParam.LazerDevIndex)
                                            {
                                                case 1:
                                                    {
                                                        GTSCard.SetDo(axisX.CardNo, (short)(9 + curPoint.LazerDocumentIndex), 0);
                                                        while (true)
                                                        {
                                                            if (GTSCard.GetDi(axisX.CardNo, 1))
                                                            {
                                                                GTSCard.SetDo(axisX.CardNo, (short)(9 + curPoint.LazerDocumentIndex), 1);
                                                                dMarkSpeed = speedLimit[curPoint.LazerDocumentIndex, 0];
                                                                break;
                                                            }
                                                            System.Threading.Thread.Sleep(100);
                                                        }
                                                    }
                                                    break;
                                                default:
                                                case 0:
                                                    {
                                                        var str = MFunc.LazerScript(systemParam.RightWeldIP, $"Start:{curPoint.LazerDocumentIndex}");
                                                        var strs = str.Split(new char[] { ':' });
                                                        if (strs.Length >= 2)
                                                        {
                                                            if (double.TryParse(strs[1], out var v))
                                                            {
                                                                dMarkSpeed = v;
                                                            }
                                                        }
                                                    }
                                                    break;
                                            }


                                        }).ContinueWith(arg => LazerDone = true);

                                    }
                                    if (pLC.Connected)
                                    {
                                        pLC.WriteYCoil(18, true);
                                    }
                                    s1 = new LineSeries { LineStyle = LineStyle.Solid, Color = OxyColors.SpringGreen, TrackerFormatString = "{4:f0}瓦" };
                                    plotIndex = 0;
                                    stepnum = 1109;
                                }
                            }
                            break;
                        case 1109:
                            if (!PauseFlag)
                            {
                                if ((systemParam.IsSimulate && sw.Elapsed.TotalMilliseconds > 600) || (!systemParam.IsSimulate && LazerDone))
                                {
                                    PlotModel1.Series.Clear();
                                    PlotModel1.Series.Add(s1);
                                    PlotModel1.InvalidatePlot(true);
                                    sw.Stop();
                                    if (pLC.Connected)
                                    {
                                        pLC.WriteYCoil(18, false);
                                    }
                                    LazerDone = false;
                                    double lzH = Math.Round(GTSCard.GetEnc(0, 10) / 1000, 3);//测高
                                    lzDist = Math.Round(mbornH - lzH, 3);//焦距
                                    addMessage($"点{curPoint.Name}:功率:{ipg_power} W,速度:{dMarkSpeed} mm/s,测高:{lzH}mm,焦距:{lzDist}mm,视觉:{vresult}:ΔX:{vx1:f3}mm,ΔY:{vy1:f3}mm");
                                    System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                                    {
                                        aggregator.SendMessage($"Select;{curPoint.Id}", "Run");
                                    }));
                              
                                    stepnum = 1110;
                                }
                                else
                                {
                                    var adcv = GTSCard.GetAdc(2);
                                    var powercur = Math.Round(adcv / 4 * 1000 - systemParam.RightPowerBase, 0);
                                    if (powercur > ipg_power)
                                    {
                                        ipg_power = powercur;
                                    }
                                    s1.Points.Add(new DataPoint(plotIndex++, powercur));
                                }
                            }
                            break;
                        case 1110:
                            if (!PauseFlag)
                            {
                                double acc = axisZ.Acc * systemParam.AccMul;
                                if (mz2UpDist < 15)
                                {
                                    mz2UpDist = 15;
                                }
                                GTSCard.AxisPosMove(ref axisZ, curPoint.Z + mz2UpDist, mzRunSpeed, acc);
                                stepnum = 1111;
                            }
                            break;
                        case 1111:
                            if (!PauseFlag)
                            {
                                if ((finishCount < weldPointsCount - 1 && (GTSCard.GetEnc(axisZ) - curPoint.Z) > mz2UpDist / 3 * 2) || (finishCount >= weldPointsCount - 1 && GTSCard.AxisPosMoveCheckDone(axisZ)))
                                {                                   
                                    //上下限报警弹框
                                    if (!systemParam.IsSimulate && (ipg_power > powerLimit[curPoint.LazerDocumentIndex, 1] || ipg_power < powerLimit[curPoint.LazerDocumentIndex, 2] ||
                                        dMarkSpeed > speedLimit[curPoint.LazerDocumentIndex, 1] || dMarkSpeed < speedLimit[curPoint.LazerDocumentIndex, 2] ||
                                        lzDist > focalLengthLimit[curPoint.LazerDocumentIndex, 1] || lzDist < focalLengthLimit[curPoint.LazerDocumentIndex, 2]))
                                    {
                                        string alarmstr = $"{curPoint.Name}:上下限报警!\n";

                                        if (ipg_power > powerLimit[curPoint.LazerDocumentIndex, 1])
                                        {
                                            alarmstr += $"功率超上限:{ipg_power}W > {powerLimit[curPoint.LazerDocumentIndex, 1]}W\n";
                                        }
                                        if (ipg_power < powerLimit[curPoint.LazerDocumentIndex, 2])
                                        {
                                            alarmstr += $"功率超下限:{ipg_power}W < {powerLimit[curPoint.LazerDocumentIndex, 2]}W\n";
                                        }

                                        if (dMarkSpeed > speedLimit[curPoint.LazerDocumentIndex, 1])
                                        {
                                            alarmstr += $"速度超上限:{dMarkSpeed}mm/s > {speedLimit[curPoint.LazerDocumentIndex, 1]}mm/s\n";
                                        }
                                        if (dMarkSpeed < speedLimit[curPoint.LazerDocumentIndex, 2])
                                        {
                                            alarmstr += $"速度超下限:{dMarkSpeed}mm/s < {speedLimit[curPoint.LazerDocumentIndex, 2]}mm/s\n";
                                        }

                                        if (lzDist > focalLengthLimit[curPoint.LazerDocumentIndex, 1])
                                        {
                                            alarmstr += $"焦距超上限:{lzDist}mm > {focalLengthLimit[curPoint.LazerDocumentIndex, 1]}mm\n";
                                        }
                                        if (lzDist < focalLengthLimit[curPoint.LazerDocumentIndex, 2])
                                        {
                                            alarmstr += $"焦距超下限:{lzDist}mm < {focalLengthLimit[curPoint.LazerDocumentIndex, 2]}mm\n";
                                        }
                                        imageFailDone = false;
                                        if (pLC.Connected)
                                        {
                                            pLC.WriteMCoil(2561, true);
                                        }
                                        addMessage(alarmstr);
                                        System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                                        {
                                            DialogParameters param = new DialogParameters();
                                            param.Add("Message", alarmstr);
                                            _dialogService.Show("MessageAlarmDialog", param, arg =>
                                            {
                                                imageFailDone = true;
                                            });
                                        }));
                                        stepnum = 1112;
                                    }
                                    else
                                    {
                                        stepnum = 1120;
                                    }
                                    if (!systemParam.IsSimulate)
                                    {
                                        #region 保存CSV
                                        var _record = new Comparison
                                        {
                                            条码 = GetBarcode(curPoint.ProductIndex),
                                            开始时间 = DateTime.Now,
                                            功率 = ipg_power,
                                            功率标准 = powerLimit[curPoint.LazerDocumentIndex, 0],
                                            功率上限 = powerLimit[curPoint.LazerDocumentIndex, 1],
                                            功率下限 = powerLimit[curPoint.LazerDocumentIndex, 2],
                                            速度 = dMarkSpeed,
                                            速度标准 = speedLimit[curPoint.LazerDocumentIndex, 0],
                                            速度上限 = speedLimit[curPoint.LazerDocumentIndex, 1],
                                            速度下限 = speedLimit[curPoint.LazerDocumentIndex, 2],
                                            焦距 = lzDist,
                                            焦距标准 = focalLengthLimit[curPoint.LazerDocumentIndex, 0],
                                            焦距上限 = focalLengthLimit[curPoint.LazerDocumentIndex, 1],
                                            焦距下限 = focalLengthLimit[curPoint.LazerDocumentIndex, 2],
                                            测高 = mbornH - lzDist,
                                            焊接点 = curPoint.Name
                                        };
                                        try
                                        {
                                            string dirpath = Path.Combine(comparisonPath, systemParam.PartList[systemParam.PartIndex], "Comparison_R");
                                            Directory.CreateDirectory(dirpath);
                                            string filename = Path.Combine(dirpath, DateTime.Now.ToString("yyyyMMdd") + "_R.csv");
                                            if (!File.Exists(filename))
                                            {
                                                using (var writer = new StreamWriter(filename))
                                                using (var csv = new CsvWriter(writer, CultureInfo.InvariantCulture))
                                                {
                                                    csv.WriteHeader<Comparison>();
                                                    csv.NextRecord();
                                                }
                                            }
                                            using (var stream = File.Open(filename, FileMode.Append))
                                            using (var writer = new StreamWriter(stream))
                                            using (var csv = new CsvWriter(writer, CultureInfo.InvariantCulture))
                                            {
                                                csv.WriteRecord(_record);
                                                csv.NextRecord();
                                            }
                                        }
                                        catch (Exception ex)
                                        {
                                            addMessage(ex.Message);
                                        }
                                        #endregion
                                        #region 保存MES
                                        string jsonString = JsonConvert.SerializeObject(_record, Formatting.Indented);
                                        aggregator.SendMessage(jsonString, "MES2");
                                        #endregion
                                    }
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 1110;
                            }
                            break;
                        case 1112:
                            if (!PauseFlag)
                            {
                                if (imageFailDone)
                                {
                                    imageFailDone = false;
                                    if (pLC.Connected)
                                    {
                                        pLC.WriteMCoil(2561, false);
                                    }
                                    stepnum = 1120;
                                }
                            }
                            break;
                        case 1120:
                            if (!PauseFlag)
                            {
                                finishCount++;
                                aggregator.SendMessage($"FinishCount2;{finishCount}", "Run");
                                stepnum = 1101;
                            }
                            break;
                        case 1200:
                            if (!PauseFlag)
                            {
                                stepnum = 1201;
                            }
                            break;
                        case 1201:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisX, initPoint.X, mxYRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, initPoint.Y, mxYRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ, initPoint.Z, mzRunSpeed);
                                stepnum = 1202;
                            }
                            break;
                        case 1202:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY)
                                    && GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    stepnum = 1203;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 1201;
                            }
                            break;
                        case 1203:
                            if (!PauseFlag)
                            {
                                if (otherFinishCount == 999)
                                {
                                    addMessage("工作结束");
                                    aggregator.SendMessage("InitFinish", "Run");
                                    pLC.WriteMCoil(2579, true);
                                    stepnum = 9;
                                }
                            }
                            break;
                        #endregion
                        default:
                            break;
                    }
                    #endregion

                }
                catch { }
                #region 更新界面
                if (cycleCount++ > 100)
                {
                    cycleCount = 0;
                    StepNum = stepnum;
                }
                #endregion
                System.Threading.Thread.Sleep(10);
            }
        }
        void LoadParam()
        {
            try
            {
                //Json序列化，从文件读取
                string jsonString = File.ReadAllText(System.IO.Path.Combine(System.AppDomain.CurrentDomain.BaseDirectory, "Param.json"));
                systemParam = JsonConvert.DeserializeObject<Param>(jsonString);
            }
            catch (Exception ex)
            {
                addMessage(ex.Message);
            }
        }
        void LoadParamFromDB()
        {
            try
            {
                using var mdb = new MDbContext(systemParam.PartList[systemParam.PartIndex]);
                var _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "XYRunSpeed");
                if (_mParam != null)
                {
                    mxYRunSpeed = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "ZRunSpeed");
                if (_mParam != null)
                {
                    mzRunSpeed = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "ZSafePos" && m.Hand == 1);
                if (_mParam != null)
                {
                    mz2SafePos = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "ZUpDist" && m.Hand == 1);
                if (_mParam != null)
                {
                    mz2UpDist = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "LineScanSpeed");
                if (_mParam != null)
                {
                    mlineScanSpeed = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "BornH" && m.Hand == 1);
                if (_mParam != null)
                {
                    mbornH = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(p => p.Name == "PointInit" && p.Hand == 1);
                if (_mParam != null)
                {
                    initPoint = JsonConvert.DeserializeObject<MPoint>(_mParam.Value);
                }

                _mParam = mdb.MParams.FirstOrDefault(p => p.Name == "PreDelay");
                if (_mParam != null)
                {
                    preDelay = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(p => p.Name == "XMinDist");
                if (_mParam != null)
                {
                    mXMinDist = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(p => p.Name == "VxyLimit");
                if (_mParam != null)
                {
                    vxyLimit = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(p => p.Name == "FlyPreDelay");
                if (_mParam != null)
                {
                    flyPreDelay = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(p => p.Name == "ComparisonPath");
                if (_mParam != null)
                {
                    comparisonPath = _mParam.Value;
                }
                _mParam = mdb.MParams.FirstOrDefault(p => p.Name == "ImagePath");
                if (_mParam != null)
                {
                    imagePath = _mParam.Value;
                }
                _mParam = mdb.MParams.FirstOrDefault(p => p.Name == "FlyGrabPreDelay" && p.Hand == 1);
                if (_mParam != null)
                {
                    mFlyGrabPreDelay = double.Parse(_mParam.Value);
                }
               
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "ZLimit");
                if (_mParam != null)
                {
                    mZLimit = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "CameraGrabZPos" && m.Hand == 1);
                if (_mParam != null)
                {
                    mCameraGrabZPos = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "ZDistUpLimit");
                if (_mParam != null)
                {
                    zDistUpLimit = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "ZDistLowLimit");
                if (_mParam != null)
                {
                    zDistLowLimit = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "IsVisionResultConfirm");
                if (_mParam != null)
                {
                    if (bool.TryParse(_mParam.Value, out var v1))
                    {
                        isVisionResultConfirm = v1;
                    }
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "IsDoubleAlBarControl");
                if (_mParam != null)
                {
                    if (bool.TryParse(_mParam.Value, out var v1))
                    {
                        isDoubleAlBarControl = v1;
                    }
                }
                var _limitParams = mdb.ParamScales.Where(p => p.Hand == 1);
                if (_limitParams.Any())
                {
                    for (int i = 0; i < 5; i++)
                    {
                        var _power = _limitParams.FirstOrDefault(v => v.Name == "Power" && v.LazerDocumentIndex == i);
                        if (_power != null)
                        {
                            powerLimit[i, 0] = _power.PValue;
                            powerLimit[i, 1] = _power.UpLimit;
                            powerLimit[i, 2] = _power.DownLimit;
                        }
                        var _speed = _limitParams.FirstOrDefault(v => v.Name == "Speed" && v.LazerDocumentIndex == i);
                        if (_speed != null)
                        {
                            speedLimit[i, 0] = _speed.PValue;
                            speedLimit[i, 1] = _speed.UpLimit;
                            speedLimit[i, 2] = _speed.DownLimit;
                        }
                        var _focalLength = _limitParams.FirstOrDefault(v => v.Name == "FocalLength" && v.LazerDocumentIndex == i);
                        if (_focalLength != null)
                        {
                            focalLengthLimit[i, 0] = _focalLength.PValue;
                            focalLengthLimit[i, 1] = _focalLength.UpLimit;
                            focalLengthLimit[i, 2] = _focalLength.DownLimit;
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                addMessage(ex.Message);
            }
        }
        void LoadCameraParam()
        {
            try
            {
                string calibfilepath1 = "Camera\\Calib\\Left";
                string calibfilepath2 = "Camera\\Calib\\Right";

                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, calibfilepath1, "CalibHomMat2D.tup"), out calibHomMat2D1);

                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, calibfilepath2, "CalibHomMat2D.tup"), out calibHomMat2D2);

            }
            catch (Exception ex)
            {
                addMessage(ex.Message);
            }
        }        
        private void XHome(CancellationToken token)
        {
            int stepnum = 0;
            var axisX = gTSCard.A5;
            int actionCount = 0;
            addMessage("X轴回原点中");
            while (true)
            {
                if (token.IsCancellationRequested)
                {
                    return;
                }
                switch (stepnum)
                {
                    case 0:
                        if (!XHomed)
                        {
                            stepnum = 3;
                        }
                        else
                        {
                            addMessage("X轴已回原点");
                            return;
                        }
                        break;
                    case 3:
                        if (!PauseFlag)
                        {
                            GTSCard.AxisHomeMove(axisX);
                            stepnum = 4;
                        }
                        break;

                    case 4:
                        if (!PauseFlag)
                        {
                            if (GTSCard.AxisHomeCheckDone(axisX))
                            {
                                actionCount++;
                                if (actionCount > 1)
                                {
                                    System.Threading.Thread.Sleep(2000);
                                    GTSCard.AxisZeroSet(axisX);
                                    GTSCard.AxisPosSet(axisX, systemParam.RightDelta.X);
                                    GTSCard.AxisEncSet(axisX, systemParam.RightDelta.X);
                                    GTSCard.ClearAlm1(axisX);
                                    XHomed = true;
                                    addMessage("X轴回原点完成");
                                    return;
                                }
                                else
                                {
                                    GTSCard.AxisZeroSet(axisX);
                                    GTSCard.ClearAlm1(axisX);
                                    stepnum = 3;
                                }
                            }
                        }
                        else
                        {
                            GTSCard.AxisStop(axisX, 0);
                            stepnum = 3;
                        }
                        break;
                    default:
                        break;
                }
                System.Threading.Thread.Sleep(100);
            }
        }
        private void YHome(CancellationToken token)
        {
            int stepnum = 0;
            var axisY = gTSCard.A6;
            int actionCount = 0;
            addMessage("Y轴回原点中");
            while (true)
            {
                if (token.IsCancellationRequested)
                {
                    return;
                }
                switch (stepnum)
                {
                    case 0:
                        if (!YHomed)
                        {
                            stepnum = 3;
                        }
                        else
                        {
                            addMessage("Y轴已回原点");
                            return;
                        }
                        break;
                    case 3:
                        if (!PauseFlag)
                        {
                            GTSCard.AxisHomeMove(axisY);
                            stepnum = 4;
                        }
                        break;
                    case 4:
                        if (!PauseFlag)
                        {
                            if (GTSCard.AxisHomeCheckDone(axisY))
                            {
                                actionCount++;
                                if (actionCount > 1)
                                {
                                    System.Threading.Thread.Sleep(2000);
                                    GTSCard.AxisZeroSet(axisY);
                                    GTSCard.AxisPosSet(axisY, systemParam.RightDelta.Y);
                                    GTSCard.AxisEncSet(axisY, systemParam.RightDelta.Y);
                                    GTSCard.ClearAlm1(axisY);
                                    YHomed = true;
                                    addMessage("Y轴回原点完成");
                                    return;
                                }
                                else
                                {
                                    GTSCard.AxisZeroSet(axisY);
                                    GTSCard.ClearAlm1(axisY);
                                    stepnum = 3;
                                }
                            }
                        }
                        else
                        {
                            GTSCard.AxisStop(axisY, 0);
                            stepnum = 3;
                        }
                        break;
                    default:
                        break;
                }
                System.Threading.Thread.Sleep(100);
            }
        }
        private void ResetAction()
        {
            GTSCard.AxisStop(gTSCard.A5, 0);
            GTSCard.AxisStop(gTSCard.A6, 0);
            GTSCard.AxisStop(gTSCard.A7, 0);
            GTSCard.ServoOff(gTSCard.A5);
            GTSCard.ServoOff(gTSCard.A6);
            GTSCard.ServoOff(gTSCard.A7);
            GTSCard.SetDo(gTSCard.A1.CardNo, 1, 1);
            GTSCard.SetDo(gTSCard.A1.CardNo, 9, 1);
            GTSCard.SetDo(gTSCard.A1.CardNo, 10, 1);
            GTSCard.SetDo(gTSCard.A1.CardNo, 11, 1);
            GTSCard.SetDo(gTSCard.A1.CardNo, 12, 1);
            GTSCard.SetDo(gTSCard.A1.CardNo, 13, 1);
            GTSCard.SetDo(gTSCard.A1.CardNo, 14, 0);
            GTSCard.SetDo(gTSCard.A1.CardNo, 15, 0);
            if (pLC.Connected)
            {
                pLC.WriteYCoil(18, false);
                pLC.WriteYCoil(19, false);
            }
        }
        private void addMessage(string str)
        {
            logger.Debug("R:" + str);
            string[] s = MessageStr.Split('\n');
            if (s.Length > 1000)
            {
                MessageStr = "";
            }
            if (MessageStr != "")
            {
                MessageStr += "\n";
            }
            MessageStr += DateTime.Now.ToString("yyyy/MM/dd HH:mm:ss") + " " + str;
        }
        private string GetBarcode(int productIndex)
        {
            if (getBarcodeService.Barcodes.Length > productIndex)
            {
                return getBarcodeService.Barcodes[productIndex].Substring(0);
            }
            else
            {
                return string.Empty;
            }
        }
        private void VisionAction(string procedureName, HObject img, HTuple dir, out HTuple row,
        out HTuple column, out HTuple angle, out HTuple result)
        {
            result = new HTuple(0);
            try
            {
                string[] strs = procedureName.Split("_");
                int id = int.Parse(strs[0]);
                HOperatorSet.RotateImage(img, out var ho_ImageRotate, (90 * dir) * -1, "constant");
                HOperatorSet.GetImageSize(ho_ImageRotate, out var hv_Width, out var hv_Height);
                HTuple hv_row = new HTuple(0), hv_column = new HTuple(0), hv_angle = new HTuple(0), hv_score = new HTuple(0), v1 = new HTuple(0), v2 = new HTuple(0);
                HObject eImage = ho_ImageRotate;
                HObject ngRegion, SelectedRegion;
                HOperatorSet.GenEmptyObj(out ngRegion);
                HOperatorSet.GenEmptyObj(out SelectedRegion);
                bool r1 = true;
                using var mdb = new MDbContext(systemParam.PartList[systemParam.PartIndex]);
                var projects = mdb.VisionProjects.Where(pro => pro.ProgramId == id).OrderBy(pro => pro.PcsOrder);
                if (projects.Any())
                {
                    var mprojects = projects.ToList();
                    foreach (var project in mprojects)
                    {
                        if (!r1)
                        {
                            break;
                        }
                        var _function = mdb.VisionFunctions.FirstOrDefault(func => func.Id == project.FunctionId);
                        if (_function != null)
                        {
                            r1 = MFunc.FunDictionary[_function.Name].Run(ref eImage, project.Parameter, ref hv_row, ref hv_column, ref hv_angle, ref hv_score, ref ngRegion, ref SelectedRegion, ref v1, ref v2);
                        }
                        else
                        {
                            r1 = false;
                        }
                    }

                }
                if (r1)
                {
                    result = new HTuple(1);
                    row = hv_row;
                    column = hv_column;
                    angle = hv_angle;
                    switch (dir.I)
                    {
                        case 1://90°
                            {
                                var box = row;
                                row = hv_Width - 1 - column;
                                column = box;
                            }
                            break;
                        case 2://180°
                            {
                                var box1 = row;
                                var box2 = column;
                                row = box1 * -1 + hv_Height - 1;
                                column = box2 * -1 + hv_Width - 1;
                            }
                            break;
                        case 3://270°
                            {
                                var box1 = hv_Width - 1 - column;
                                var box2 = row;
                                row = box1 * -1 + hv_Width - 1;
                                column = box2 * -1 + hv_Height - 1;
                            }
                            break;
                        default:
                            break;
                    }
                }
                else
                {
                    result = new HTuple(0);
                    row = new HTuple(0);
                    column = new HTuple(0);
                    angle = new HTuple(0);
                }
                eImage.Dispose();
            }
            catch (Exception ex)
            {
                logger.Error(ex);
                result = new HTuple(0);
                row = new HTuple(0);
                column = new HTuple(0);
                angle = new HTuple(0);
            }

        }
        #endregion
    }
}
